#include <Functions.h>
#include <FloatArithmetic.h>

static void Convolution(float* convolution, float* spectrum);
static byte FFT(float* signal, float* signal_img);
static float GetHerz(float freq);	

#define MELSCALE_DELTA 50.789978f
static const float MelScalePoints[] = { 0.0f, 50.789978f, 101.579956f, 152.369934f, 203.159912f,
										253.949890f, 304.739868f, 355.529846f, 406.319824f,
										457.109802f, 507.899780f, 558.689758f, 609.479736f,
										660.269714f, 711.059692f, 761.849670f, 812.639648f,
										863.429626f, 914.219604f, 965.009583f		
									};

#define INTEGER_CONSTANT 700
#define FLOAT_CONSTANT 1127.01048			//magical number with floating point, as we like;

#if FURIE_MEMBERS_MAX == 4096
	#define FREQ_STEP 0.299036121f
#endif

#if FURIE_MEMBERS_MAX == 2048
	#define FREQ_STEP 0.149527979f
#endif

#if FURIE_MEMBERS_MAX == 1024
	#define FREQ_STEP 0.0747664692f
#endif

static const float Rcoef[14] =
{  
	-1.0000000000000000F,  0.0000000000000000F,  0.7071067811865475F,
	0.9238795325112867F,  0.9807852804032304F,  0.9951847266721969F,
	0.9987954562051724F,  0.9996988186962042F,  0.9999247018391445F,
	0.9999811752826011F,  0.9999952938095761F,  0.9999988234517018F,
	0.9999997058628822F,  0.9999999264657178F
};
  
static const float Icoef[14] =
{  
	 0.0000000000000000F, -1.0000000000000000F, -0.7071067811865474F,
	-0.3826834323650897F, -0.1950903220161282F, -0.0980171403295606F,
	-0.0490676743274180F, -0.0245412285229122F, -0.0122715382857199F,
	-0.0061358846491544F, -0.0030679567629659F, -0.0015339801862847F,
	-0.0007669903187427F, -0.0003834951875714F
};
 
void ProcessSignal(float* signal, float* signal_img, uint size, float* result)
{
	FFT(signal, signal_img);
	Convolution(result, signal);
}
 
byte FFT(float* signal, float* signal_img)
{
	register int  i, j, n, io, _count, _count_next;
	float         ru, iu, rtp, itp, rtq, itq, rw, iw, sr;
 
	register float* Rdat = signal;
	register float* Idat = signal_img;

	_count = FURIE_MEMBERS_MAX;
	
	for(n = 1; n <= LOG_FURIE_MEMBERS_MAX; n++)
	{
		rw = Rcoef[LOG_FURIE_MEMBERS_MAX - n];
		iw = Icoef[LOG_FURIE_MEMBERS_MAX - n];

		_count_next = _count >> 1;
		
		ru = 1.0F;
		iu = 0.0F;

		for(j=0; j<_count_next; j++)
		{
			for(i = j; i < FURIE_MEMBERS_MAX; i += _count)
			{
				io = i + _count_next;
				rtp = Rdat[i] + Rdat[io];
				itp = Idat[i] + Idat[io];
				rtq = Rdat[i] - Rdat[io];
				itq = Idat[i] - Idat[io];
			
				Rdat[i] = rtp;
				Idat[i] = itp;

				Rdat[io] = rtq * ru - itq * iu;
				Idat[io] = itq * ru + rtq * iu;
			}
			
			sr = ru;
			ru = ru * rw - iu * iw;
			iu = iu * rw + sr * iw;
		}
		
		_count >>= 1;
	}
 
	for(j = i = 1; i < FURIE_MEMBERS_MAX; i++)
	{
		if(i < j)
		{
			io = i - 1;
			_count_next = j - 1;
		
			rtp = Rdat[_count_next];
			itp = Idat[_count_next];
			
			Rdat[_count_next] = Rdat[io];
			Idat[_count_next] = Idat[io];
			
			Rdat[io] = rtp;
			Idat[io] = itp;
		}
 
		n = HALF_FURIE_MEMBERS_MAX;
 
		while(n < j)
		{
			j   = j - n;
			n >>= 1;
		}

		j = j + n;
	}
	
	for(i = 0; i< FURIE_MEMBERS_MAX; i++)
		Rdat[i] = Rdat[i]*Rdat[i];
 
	return true;
}
 
void Convolution(float* convolution, float* spectrum)
{
	register uint i = 0, j = 0;
	
	float pre_point = MelScalePoints[0];
	float central_point = MelScalePoints[1];
	float post_point = 0;

	float freq_j = 0;
	float window = 0;

	for(i = 1; i < WINDOW_COUNT - 1; i++)
	{
		post_point = MelScalePoints[i + 1];
	
		freq_j = 0;
		convolution[i-1] = 0;
		
		for(j = 0; j < FURIE_MEMBERS_MAX; j++)
		{
			if(freq_j <= pre_point)
			{
				window = 0;

				freq_j += FREQ_STEP;
				continue;
			}

			if(freq_j > pre_point && freq_j <= central_point)
				window = (freq_j - pre_point)/MELSCALE_DELTA;


			if(freq_j > central_point && freq_j <= post_point)
				window = (post_point - freq_j)/MELSCALE_DELTA;

			if(freq_j > post_point)
				break;
			
			convolution[i-1] += spectrum[j]*GetHerz(window);
		
			freq_j += FREQ_STEP;
		}
		
		pre_point = central_point;
		central_point = post_point;
	}
}

float GetHerz(float freq)
{
	freq = freq / FLOAT_CONSTANT; 
	freq = Exp(freq);
	freq = freq * INTEGER_CONSTANT;

	return freq;
}